import rclpy
from rclpy.node import Node
import rclpy.qos
from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2
from rclpy.parameter import Parameter

class PointCloudExtractor(Node):
    def __init__(self):
        super().__init__('point_cloud_extractor')

        self.declare_parameter('calculated_obstacles')           # 创建一个参数，并设置参数的默认值

        qos_profile = rclpy.qos.QoSProfile(
        reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, 
        history=rclpy.qos.HistoryPolicy.KEEP_LAST,              
        depth=10
        )
        self.subscription = self.create_subscription(
            PointCloud2,
            '/octomap_point_cloud_centers',
            self.point_cloud_callback,
            qos_profile
        )
        # 打印启动日志
        self.get_logger().info("'point_cloud_extractor' node has been started!")
    
    def point_cloud_callback(self, msg):
        # 使用 sensor_msgs_py.point_cloud2 来解析 PointCloud2 消息
        # 将消息中的数据解码为一个生成器
        points = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)

        # 将坐标 (x, y, z) 存储到一个列表中
        point_list = []
        i_num = 0
        for point in points:
            x, y, z = point
            # if z<0 or z>5:
            #     continue
            # i_num += 1
            # if i_num>30:
            #     break
            point_list.append(float(x))
            point_list.append(float(y))
            point_list.append(float(z))
        lenth = len(point_list)
        obstacle_positions_param = Parameter('calculated_obstacles',  # 重新将参数值设置为指定值
                            Parameter.Type.DOUBLE_ARRAY, point_list)
        self.set_parameters([obstacle_positions_param])                    # 将重新创建的参数列表发送给ROS2系统
        # 打印点云坐标
        self.get_logger().info(f"Extracted Points: {point_list[:10]}...   length: {lenth}")  # 打印前10个点的坐标

def main(args=None):
    rclpy.init(args=args)
    node = PointCloudExtractor()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
